(* Content-type: application/mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 7.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       145,          7]
NotebookDataLength[    132846,       3836]
NotebookOptionsPosition[    131625,       3800]
NotebookOutlinePosition[    132017,       3817]
CellTagsIndexPosition[    131974,       3814]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(*", "AMBER", "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/AMBER_Output_Test/model\>\"", "]"}], 
    ";"}], "\n", 
   RowBox[{"<<", "RobotLinks.m"}], "\n", 
   RowBox[{"<<", "Linearize.m"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"SetDirectory", "[", 
      RowBox[{
      "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
       "\"\<build_torso\>\""}], "]"}], ";"}], "*)"}]}]}]], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"LineNumber", " ", "=", " ", "2"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"IndexAllPre", " ", "=", " ", 
    RowBox[{"Import", "[", 
     RowBox[{"\"\<../data/IndexAll.mat\>\"", ",", "\"\<MAT\>\""}], "]"}]}], 
   ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Dimensions", "[", "IndexAllPre", "]"}], ";"}], 
   "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   RowBox[{"IndexAll", "=", 
    RowBox[{"Join", "[", 
     RowBox[{"First", "[", "IndexAllPre", "]"}], "]"}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"Dimensions", "[", "IndexAll", "]"}], ";"}], "*)"}], 
  "\[IndentingNewLine]"}], "\n", 
 RowBox[{
  RowBox[{"HipPosIndex", "=", 
   RowBox[{"Round", "[", 
    RowBox[{"IndexAll", "[", 
     RowBox[{"[", 
      RowBox[{"LineNumber", ",", "1"}], "]"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"NSslopeIndex", "=", 
   RowBox[{"Round", "[", 
    RowBox[{"IndexAll", "[", 
     RowBox[{"[", 
      RowBox[{"LineNumber", ",", "2"}], "]"}], "]"}], "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"TorsoIndex", "=", 
   RowBox[{"Round", "[", 
    RowBox[{"IndexAll", "[", 
     RowBox[{"[", 
      RowBox[{"LineNumber", ",", "3"}], "]"}], "]"}], "]"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, 
   3.51895445209264*^9, {3.519676671577675*^9, 3.519676676594407*^9}, {
   3.5239667982730017`*^9, 3.523966813334333*^9}, {3.523967028425743*^9, 
   3.523967032289135*^9}, {3.523967158902598*^9, 3.523967166280966*^9}, {
   3.52396837941966*^9, 3.523968408653638*^9}, {3.523968885862918*^9, 
   3.5239689132702827`*^9}, {3.523968985792946*^9, 3.5239689955887833`*^9}, {
   3.523969165241643*^9, 3.5239692240661287`*^9}, {3.5239694880148153`*^9, 
   3.523969499785118*^9}, {3.5239695714828377`*^9, 3.523969581546866*^9}, 
   3.52396965474053*^9, {3.523979540006937*^9, 3.523979540390903*^9}, 
   3.5243321051012077`*^9, {3.524333648683949*^9, 3.524333648941087*^9}, 
   3.52491991104259*^9, {3.526909166246196*^9, 3.5269091768319893`*^9}, {
   3.527022421428713*^9, 3.527022438914948*^9}, {3.527022726620986*^9, 
   3.5270227402575073`*^9}, 3.527027652455699*^9, {3.527027687003477*^9, 
   3.527027691012311*^9}, {3.527031346610958*^9, 3.527031347212298*^9}, {
   3.527075803741696*^9, 3.527075873329626*^9}, 3.5270759173702497`*^9, {
   3.527078036507852*^9, 3.527078050126071*^9}, {3.527197911583006*^9, 
   3.527197913230167*^9}, {3.527266742519038*^9, 3.527266742600219*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{"t0", "=", 
    RowBox[{"AbsoluteTime", "[", "]"}]}], ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"model", " ", "specification"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"robotInfo", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{"{", 
      RowBox[{
      "Lc", ",", "Lt", ",", "LT", ",", "rc", ",", "rt", ",", "rT", ",", "mh", 
       ",", "mt", ",", "mc", ",", "g"}], "}"}], ",", "\[IndentingNewLine]", 
     RowBox[{"{", 
      RowBox[{
      "347.98", ",", "261.112", ",", "0", ",", "282.37", ",", "128.15", ",", 
       "9.97", ",", 
       StyleBox["804.83",
        FontWeight->"Plain"], ",", "606.15", ",", "213.79", ",", "9810"}], 
      "}"}]}], "}"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"constsubs", "=", 
   RowBox[{
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       RowBox[{"#1", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
       "\[Rule]", 
       RowBox[{"Rationalize", "[", 
        FractionBox[
         RowBox[{"#1", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}],
          "1000"], "]"}]}], "&"}], ")"}], "/@", 
    RowBox[{"Transpose", "[", "robotInfo", "]"}]}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ndof", " ", "=", " ", "5"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"mm", "=", 
   RowBox[{
    RowBox[{"{", 
     RowBox[{"mc", ",", "mt", ",", "mh", ",", "mt", ",", "mc"}], "}"}], "/.", 
    "\[InvisibleSpace]", "constsubs"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"statesubs", "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", "i", "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}], ",", 
      RowBox[{"Table", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SuperscriptBox[
           SubscriptBox["\[Theta]", "i"], "\[Prime]",
           MultilineFunction->None], "[", "t", "]"}], "\[Rule]", 
         RowBox[{"x", "[", 
          RowBox[{"i", "+", "ndof"}], "]"}]}], ",", 
        RowBox[{"{", 
         RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], "]"}]}], 
   ";"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"define", " ", "inertia", " ", "matrices"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"For", "[", 
    RowBox[{
     RowBox[{"i", "=", "1"}], ",", 
     RowBox[{"i", "\[LessEqual]", 
      RowBox[{"Length", "[", "mm", "]"}]}], ",", 
     RowBox[{"i", "++"}], ",", 
     RowBox[{
      SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
      RowBox[{
       RowBox[{"mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
       " ", 
       RowBox[{"DiagonalMatrix", "[", 
        RowBox[{"{", 
         RowBox[{"1", ",", "1", ",", "1"}], "}"}], "]"}]}]}]}], "]"}], ";"}], 
  "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"add", " ", "inertia", " ", "for", " ", "torso", " ", 
     RowBox[{"(", 
      RowBox[{"2.5", " ", "cm", " ", "radius"}], ")"}]}], ",", 
    RowBox[{"the", " ", "x"}], ",", 
    RowBox[{
    "z", " ", "terms", " ", "will", " ", "not", " ", "show", " ", "up", " ", 
     "in", " ", "the", " ", "EOM", " ", "for", " ", "this", " ", "2", "d", 
     " ", "model"}]}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "1"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", "12.37"},
         {"0", "1946798.09", 
          RowBox[{"-", "19.96"}]},
         {"12.37", 
          RowBox[{"-", "19.96"}], "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "2"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", "58.80", "2024.23"},
         {"58.80", "6396011.40", "146697.91"},
         {"2024.23", "146697.91", "418371.07"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "3"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"3730232.81", 
          RowBox[{"-", "7.44"}], "1948.96"},
         {
          RowBox[{"-", "7.44"}], "518271.83", 
          RowBox[{"-", "1348.28"}]},
         {"1948.96", 
          RowBox[{"-", "1348.28"}], "3577190.08"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "4"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"6494948.89", 
          RowBox[{"-", "60.11"}], "1985.63"},
         {
          RowBox[{"-", "60.11"}], "6396011.38", 
          RowBox[{"-", "146697.91"}]},
         {"1985.63", 
          RowBox[{"-", "146697.91"}], "418371.06"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   SubscriptBox["\[ScriptCapitalI]", "5"], "=", 
   RowBox[{"Rationalize", "[", 
    RowBox[{
     RowBox[{
      RowBox[{"(", GridBox[{
         {"1967374.33", "0", 
          RowBox[{"-", "12.37"}]},
         {"0", "1946798.091", "19.96"},
         {
          RowBox[{"-", "12.37"}], "19.96", "119696.29"}
        }], ")"}], "/", "1000000000"}], ",", 
     RowBox[{"1", "/", "1000000000"}]}], "]"}]}], ";"}], "\n", 
 RowBox[{"For", "[", 
  RowBox[{
   RowBox[{"i", "=", "1"}], ",", 
   RowBox[{"i", "\[LessEqual]", "5"}], ",", 
   RowBox[{"i", "++"}], ",", 
   RowBox[{
    SubscriptBox["\[ScriptCapitalM]", "i"], "=", 
    RowBox[{"Join", "[", 
     RowBox[{
      RowBox[{"Join", "[", 
       RowBox[{
        SubscriptBox["\[ScriptCapitalM]", "i"], ",", 
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", "2"}], "]"}], ",", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{"Table", "[", 
         RowBox[{"0", ",", 
          RowBox[{"{", "3", "}"}], ",", 
          RowBox[{"{", "3", "}"}]}], "]"}], ",", 
        SubscriptBox["\[ScriptCapitalI]", "i"], ",", "2"}], "]"}], ",", "1"}],
      "]"}]}]}], "]"}], "\n", 
 RowBox[{
  RowBox[{"p0", "=", 
   RowBox[{"{", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "z"], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "x"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}], ",", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["p", "z"], "'"}], "[", "t", "]"}], "\[Rule]", "0"}]}], 
    "}"}]}], ";"}]}], "Input",
 CellChangeTimes->{
  3.504875918224949*^9, {3.505063952502133*^9, 3.505063953970308*^9}, {
   3.5051489336912956`*^9, 3.5051489340032964`*^9}, 3.513531757509935*^9, {
   3.515870024222504*^9, 3.5158700252255616`*^9}, 3.5158863755610266`*^9, {
   3.5271753048660097`*^9, 3.527175316328361*^9}, {3.5271784037700644`*^9, 
   3.527178409550562*^9}, {3.527194806620797*^9, 3.527194812727977*^9}, {
   3.527194901003693*^9, 3.527194939760748*^9}, 3.527195000990027*^9, 
   3.527195065536849*^9, {3.527197843268285*^9, 3.527197851612855*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"generalized", " ", "coordinates"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"q", "=", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "}"}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"dq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "q"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"ddq", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "dq"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"qe", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}], "}"}]}], "}"}], ",", 
       "q"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"dqe", "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", "t"], "qe"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "location", " ", "and", " ", "direction", " ", "of", " ", "twists"}], 
    "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "0"], "=", 
     RowBox[{"{", 
      RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "0", ",", "0"}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "px"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"1", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "pz"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"PrismaticTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "0"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           RowBox[{"-", "1"}], ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", 
           RowBox[{"Lc", "+", "Lt"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Xi]", "q5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RevoluteTwist", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"0", ",", "0", ",", "Lc"}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"base", " ", "configuration"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "1"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           FractionBox[
            RowBox[{"-", "33"}], "100000"], ",", "rc"}], "}"}]}], "]"}], "/.",
        "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "2"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox["2", "100000"], ",", 
           FractionBox[
            RowBox[{"-", "381"}], "100000"], ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "3"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox[
            RowBox[{"-", "24"}], "100000"], ",", 
           FractionBox[
            RowBox[{"-", "4"}], "100000"], ",", 
           RowBox[{"Lc", "+", "Lt", "+", "rT"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox[
            RowBox[{"-", "24"}], "100000"], ",", 
           FractionBox[
            RowBox[{"-", "4"}], "100000"], ",", 
           RowBox[{"Lc", "+", "Lt", "+", "Lb"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "4"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{
           FractionBox["2", "100000"], ",", 
           FractionBox["381", "100000"], ",", 
           RowBox[{"Lc", "+", "rt"}]}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", 
       SubscriptBox["sl", "5"]], "[", "0", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"RPToHomogeneous", "[", 
        RowBox[{
         RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"0", ",", 
           FractionBox["33", "100000"], ",", "rc"}], "}"}]}], "]"}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "1"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "2"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "3"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "4"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "0"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[ScriptCapitalJ]", "5"], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"BodyJacobian", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", 
          SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "forward", " ", "kinematics", " ", "maps"}],
     "*)"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "1"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "1"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "2"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "2"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "3"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "3"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "4"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "4"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "5"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "px"], ",", 
           RowBox[{
            SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "pz"], ",", 
           RowBox[{
            SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", 
           SubscriptBox["sl", "5"]], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt"}]}], "}"}]}], "]"}], "/.", "constsubs"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", 
          RowBox[{"Lc", "+", "Lt", "+", "LT"}]}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "Lc"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{
      RowBox[{"RPToHomogeneous", "[", 
       RowBox[{
        RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
        RowBox[{"{", 
         RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}], "/.", 
      "constsubs"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "0"], ",", "0"}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "stk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "hip"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "torso"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";", 
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"ForwardKinematics", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q1"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q2"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q3"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q4"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{"{", 
          RowBox[{
           SubscriptBox["\[Xi]", "q5"], ",", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "/.", 
       "constsubs"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", "aniplot", "*)"}], "\n", 
   RowBox[{
    RowBox[{"pos", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"Join", "[", 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "stk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsk"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"1", ",", "3"}], "}"}], ",", 
          RowBox[{"{", "4", "}"}]}], "\[RightDoubleBracket]"}], ",", "2"}], 
       "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"calculate", " ", "center", " ", "of", " ", "mass"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
           
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{"Total", "[", "mm", "]"}], ")"}]}]}]}], ")"}], "/.", 
        "constsubs"}], "//.", "p0"}], "//", "Simplify"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "manipulator", " ", "inertia", " ", 
     "matrix"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]e", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       UnderoverscriptBox["\[Sum]", 
        RowBox[{"i", "=", "1"}], 
        RowBox[{"Length", "[", "mm", "]"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["\[ScriptCapitalJ]", "i"], "\[Transpose]"}], ".", 
        SubscriptBox["\[ScriptCapitalM]", "i"], ".", 
        SubscriptBox["\[ScriptCapitalJ]", "i"]}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "project", " ", "out", " ", "the", " ", "generalized", " ", "coordinates",
      " ", "defining", " ", "the", " ", "position", " ", "of", " ", "the", 
     " ", "stance", " ", "foot", " ", "to", " ", "obtain", " ", "the", " ", 
     "reduced", " ", "\[ScriptCapitalD]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalD]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"\[ScriptCapitalD]e", "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"3", ";;", "All"}], ",", 
         RowBox[{"3", ";;", "All"}]}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.504391169851249*^9, 3.504391191970865*^9}, {
   3.5129535326917257`*^9, 3.5129535355376797`*^9}, 3.513011937181409*^9, {
   3.513012138013254*^9, 3.513012139016991*^9}, 3.515858456069166*^9, {
   3.5158695182095623`*^9, 3.515869519316625*^9}, {3.52719507570704*^9, 
   3.527195080589549*^9}, {3.527195127333808*^9, 3.5271951339919443`*^9}, 
   3.527195290523535*^9}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"calculate", " ", "Coriolis", " ", "matrix"}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"\[ScriptCapitalC]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"InertiaToCoriolis", "[", 
       RowBox[{"\[ScriptCapitalD]", ",", 
        RowBox[{"Flatten", "[", "q", "]"}], ",", 
        RowBox[{"Flatten", "[", "dq", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "potential", " ", "energy", " ", "and", " ",
      "\[ScriptCapitalG]", " ", "matrix"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"V", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"g", " ", 
        RowBox[{
         UnderoverscriptBox["\[Sum]", 
          RowBox[{"i", "=", "1"}], 
          RowBox[{"Length", "[", "mm", "]"}]], 
         RowBox[{
          RowBox[{
          "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], " ", 
          RowBox[{
           RowBox[{
            SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
           "\[LeftDoubleBracket]", 
           RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]}]}], "/.", 
       "\[InvisibleSpace]", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalG]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{"q", ",", "1"}], "}"}]], "V"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "the", " ", "\[ScriptCapitalE]", " ", "matrix", " ", 
     "for", " ", "impact", " ", "and", " ", "the", " ", "guard"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"the", " ", "position", " ", "of", " ", "swing", " ", "foot"}], 
    "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "0", "]"}], "=", 
     RowBox[{"RPToHomogeneous", "[", 
      RowBox[{
       RowBox[{"IdentityMatrix", "[", "3", "]"}], ",", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{"ForwardKinematics", "[", 
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "px"], ",", 
          RowBox[{
           SubscriptBox["p", "x"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "pz"], ",", 
          RowBox[{
           SubscriptBox["p", "z"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q1"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q2"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q3"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q4"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{"{", 
         RowBox[{
          SubscriptBox["\[Xi]", "q5"], ",", 
          RowBox[{
           SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], ",", 
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "0", "]"}]}], "]"}], "]"}]}], ";"}], 
   "\n", 
   RowBox[{
    RowBox[{"\[ScriptCapitalE]", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "qe", "]"}], ",", "1"}], "}"}]], 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"1", ",", "3"}], "}"}], ",", "4"}], 
        "\[RightDoubleBracket]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"\[ScriptCapitalE]", "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "\[Rule]", ".2345"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "\[Rule]", ".1894"}],
        ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".293"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", ".094"}], 
       ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"-", ".210"}]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "\[Rule]", 
        ".923"}]}], "}"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"h", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "/.", 
       "\[InvisibleSpace]", "p0"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"hdot", " ", "=", "  ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", "h", "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"hip", " ", "position", " ", "related"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"1", " ", "Hip", " ", "Position"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"HipPos", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Linearized", " ", "Hip", " ", "Position"}], "*)"}], 
   "\n", 
   RowBox[{
    RowBox[{"LHipPos", " ", "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{"HipPos", ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"non", "-", 
     RowBox[{"stance", " ", "slope", " ", "related"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"nsslope", " ", "=", " ", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
       RowBox[{
        RowBox[{
         RowBox[{
          SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
        RowBox[{
         RowBox[{
          SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
         "\[LeftDoubleBracket]", 
         RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]], "/.", 
      "p0"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"2.", " ", "linearized", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope"}]}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"LinearNSslope", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nsslope", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"3.", " ", "hip", " ", "angle"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"HipAngle", " ", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
      RowBox[{
       SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"torso", " ", "related"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"1", " ", "Torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "thigh", " ", "angle"}]}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"theta4", " ", "=", " ", 
     RowBox[{
      SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"2", " ", "Torso", " ", "Hip", " ", "angle"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"TorsoHipAngle", "=", " ", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "+", 
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"3", " ", "Torso", " ", "Non"}], "-", 
     RowBox[{"stance", " ", "Slope"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"nstorso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}]}], 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["g", "nsf"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}], "-", 
       RowBox[{
        RowBox[{
         SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
        "\[LeftDoubleBracket]", 
        RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]}]]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"4", " ", "Linearized", " ", "torso", " ", "non"}], "-", 
     RowBox[{"stance", " ", "slope", " ", "angle"}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearNStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"nstorso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"5", " ", "Torso", " ", "stance", " ", "slope"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"storso", " ", "=", " ", 
     FractionBox[
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], 
      RowBox[{
       RowBox[{
        SubscriptBox["g", "torso"], "[", "\[Theta]", "]"}], 
       "\[LeftDoubleBracket]", 
       RowBox[{"3", ",", "4"}], "\[RightDoubleBracket]"}]]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "6", " ", "Linearized", " ", "Torso", " ", "stance", " ", "slope"}], 
    "*)"}], "\n", 
   RowBox[{
    RowBox[{"LinearStorso", "=", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{"storso", " ", "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{"7", " ", "COM"}], "*)"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "COM"], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          UnderoverscriptBox["\[Sum]", 
           RowBox[{"i", "=", "1"}], 
           RowBox[{"Length", "[", "mm", "]"}]], 
          RowBox[{
           RowBox[{
           "mm", "\[LeftDoubleBracket]", "i", "\[RightDoubleBracket]"}], 
           RowBox[{
            RowBox[{
             RowBox[{
              SubscriptBox["g", "i"], "[", "\[Theta]", "]"}], 
             "\[LeftDoubleBracket]", 
             RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/", 
            RowBox[{"(", 
             RowBox[{
              RowBox[{"2", "*", 
               RowBox[{"(", 
                RowBox[{"mc", "+", "mt"}], ")"}]}], "+", "mh"}], ")"}]}]}]}], 
         "/.", "constsubs"}], ")"}], "//.", "p0"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{"8", " ", "Linearized", " ", "COM"}], "*)"}], 
   RowBox[{
    RowBox[{
     SubscriptBox["Linearp", "COM"], "=", " ", 
     RowBox[{"Linearize", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["p", "COM"], "/.", "p0"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{
         RowBox[{
          RowBox[{"Subscript", "[", 
           RowBox[{"\[Theta]", ",", "i"}], "]"}], "[", "t", "]"}], ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}], ",", 
       RowBox[{"Table", "[", 
        RowBox[{"0", ",", 
         RowBox[{"{", 
          RowBox[{"i", ",", "5"}], "}"}]}], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", "\n"}]}]], "Input",
 CellChangeTimes->{
  3.50439255557097*^9, {3.51295513827149*^9, 3.512955172679001*^9}, {
   3.5129552037364693`*^9, 3.512955231006535*^9}, {3.512955263739307*^9, 
   3.5129553328924294`*^9}, {3.512992901952466*^9, 3.512992920307856*^9}, {
   3.5129940359160023`*^9, 3.512994058469393*^9}, 3.5129940928271093`*^9, {
   3.512994138765036*^9, 3.5129941583899803`*^9}, {3.51301260617554*^9, 
   3.513012607311402*^9}, {3.513012641937742*^9, 3.513012652342953*^9}, {
   3.513012738154763*^9, 3.513012748062248*^9}, 3.513012780026892*^9, {
   3.51301313424288*^9, 3.513013136853251*^9}, {3.527195306779914*^9, 
   3.527195310640623*^9}, {3.5271967036241007`*^9, 3.527196723980693*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"feedback", " ", "control"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"\[Chi]", "=", 
     RowBox[{"Join", "[", 
      RowBox[{"q", ",", "dq"}], "]"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"d\[Chi]", "=", 
     RowBox[{"D", "[", 
      RowBox[{"\[Chi]", ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"hipOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"HipPos", ",", "LHipPos"}], "}"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hip"], " ", "=", 
     RowBox[{"hipOutput", "[", 
      RowBox[{"[", "HipPosIndex", "]"}], "]"}]}], " ", ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["p", "hipdot"], "  ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["v", "hip"], "=", 
     RowBox[{
      RowBox[{"D", "[", 
       RowBox[{
        SubscriptBox["p", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}], 
    ";"}], "\n", 
   RowBox[{
    RowBox[{"fb", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         SubscriptBox["p", "hip"], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"time", "-", 
     RowBox[{"invariant", " ", 
      RowBox[{"parameterization", ":"}]}]}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"\[Sigma]y", "=", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
     RowBox[{
      FractionBox[
       RowBox[{
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "1"}], "]"}], " ", 
         RowBox[{"Cos", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "3"}], "]"}], " ", 
         RowBox[{"Sin", "[", 
          RowBox[{
           RowBox[{"a", "[", 
            RowBox[{"i", ",", "2"}], "]"}], " ", 
           RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
       RowBox[{"Exp", "[", 
        RowBox[{
         RowBox[{"a", "[", 
          RowBox[{"i", ",", "4"}], "]"}], 
         RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "5"}], "]"}]}]}], ";"}], 
   RowBox[{"(*", 
    RowBox[{"calculate", 
     FormBox[
      SubscriptBox["y", "d"],
      TraditionalForm], "and", " ", "its", " ", "derivatives"}], "*)"}], "\n",
    "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "1"}]], " ", "=", " ", 
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "1"}]], "=", 
     RowBox[{
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
      "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"d", ",", "2"}]], "=", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
    "calculate", " ", "actual", " ", "kinematics", " ", "outputs", " ", "and",
      " ", "Jacobians"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["p", "hip"], ",", "t"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"NSslopeOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], "}"}]}], 
    ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"TorsoOutput", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
      "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
       ",", "storso", ",", "LinearStorso", ",", 
       SubscriptBox["p", "COM"], ",", 
       SubscriptBox["Linearp", "COM"]}], "}"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["y", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", 
           RowBox[{"NSslopeOutput", "[", 
            RowBox[{"[", "NSslopeIndex", "]"}], "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
          RowBox[{"{", 
           RowBox[{"TorsoOutput", " ", "[", 
            RowBox[{"[", "TorsoIndex", "]"}], "]"}], "}"}]}], "}"}], "/.", 
        "constsubs"}], "/.", "p0"}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "1"}]], "]"}]}], "]"}]}], ";"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["Dy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["y", 
         RowBox[{"a", ",", "2"}]], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "1"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{
     SubscriptBox["DLfy", 
      RowBox[{"a", ",", "2"}]], "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        RowBox[{
         SubscriptBox["Dy", 
          RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}],
    "\[IndentingNewLine]"}]}]], "Input",
 CellChangeTimes->{{3.513533323577435*^9, 3.513533331092033*^9}, {
   3.513533381181343*^9, 3.513533384041068*^9}, {3.513534180527118*^9, 
   3.513534186497912*^9}, {3.513534242697953*^9, 3.513534243136149*^9}, {
   3.527196798384557*^9, 3.527196802874881*^9}, 3.52719779898011*^9}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    SubscriptBox["p", "hip"], " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["g", "hip"], "[", "\[Theta]", "]"}], 
      "\[LeftDoubleBracket]", 
      RowBox[{"1", ",", "4"}], "\[RightDoubleBracket]"}], "/.", "p0"}]}], ";",
    "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["p", "hipdot"], "  ", "=", " ", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["p", "hip"], "]"}]}], "]"}]}], ";", "\n", 
   RowBox[{"hipOutput", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{"HipPos", ",", "LHipPos", ",", "LHipPosD"}], "}"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["deltap", "hip"], " ", "=", 
    RowBox[{"hipOutput", "[", 
     RowBox[{"[", "2", "]"}], "]"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["deltap", "hipdot"], "  ", "=", " ", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["deltap", "hip"], "]"}]}], "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["deltav", "hip"], "=", 
    RowBox[{
     RowBox[{"D", "[", 
      RowBox[{
       SubscriptBox["deltap", "hip"], ",", "t"}], "]"}], "//", "Simplify"}]}],
    ";", "\n", 
   RowBox[{"fb", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
      FractionBox[
       RowBox[{
        SubscriptBox["deltap", "hip"], "-", 
        RowBox[{"p", "[", "1", "]"}]}], 
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", "\n", 
   RowBox[{"\[Sigma]y", "=", 
    RowBox[{
     RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "fb"}]}], ";"}], 
  "*)"}]], "Input",
 CellChangeTimes->{{3.520119821955995*^9, 3.5201198226710362`*^9}, {
  3.5201773456230736`*^9, 3.52017734590409*^9}, {3.527197052406234*^9, 
  3.5271970545001707`*^9}, {3.5271971130092497`*^9, 3.527197125055758*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    RowBox[{"HumanFunction", "[", "i_", "]"}], ":=", 
    RowBox[{
     FractionBox[
      RowBox[{
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"i", ",", "1"}], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"i", ",", "2"}], "]"}], " ", 
          RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}], "+", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"i", ",", "3"}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          RowBox[{"a", "[", 
           RowBox[{"i", ",", "2"}], "]"}], " ", 
          RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]}]}], 
      RowBox[{"Exp", "[", 
       RowBox[{
        RowBox[{"a", "[", 
         RowBox[{"i", ",", "4"}], "]"}], 
        RowBox[{"\[Sigma]", "[", "t", "]"}]}], "]"}]], "+", 
     RowBox[{"a", "[", 
      RowBox[{"i", ",", "5"}], "]"}]}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["y", 
     RowBox[{"d", ",", "1"}]], " ", "=", " ", 
    RowBox[{
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"1", ",", "1"}], "]"}], "\[IndentingNewLine]", 
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]]}], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fb"}]}]}], ";", "\n", 
   RowBox[{
    RowBox[{
     RowBox[{"a", "[", 
      RowBox[{"1", ",", "1"}], "]"}], "\n", 
     SubscriptBox["Dy", 
      RowBox[{"d", ",", "1"}]]}], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["y", 
        RowBox[{"d", ",", "1"}]], "]"}]}], "]"}]}], ";", 
   "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["Dy", 
     RowBox[{"d", ",", "2"}]], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       SubscriptBox["y", 
        RowBox[{"d", ",", "2"}]], "]"}]}], "]"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["DLfy", 
     RowBox[{"d", ",", "1"}]], "=", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
     "Simplify"}]}], ";", "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["DLfy", 
     RowBox[{"d", ",", "2"}]], "=", 
    RowBox[{
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"d", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "//", 
     "Simplify"}]}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{{3.515860352077612*^9, 3.5158603885236964`*^9}, {
  3.527197141988551*^9, 3.527197155523426*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{
    SubscriptBox["y", 
     RowBox[{"a", ",", "1"}]], "=", 
    RowBox[{
     SubscriptBox["deltav", "hip"], "\n", 
     RowBox[{"NSslopeOutput", " ", "=", " ", 
      RowBox[{"{", 
       RowBox[{"nsslope", ",", "LinearNSslope", ",", "HipAngle"}], 
       "}"}]}]}]}], ";", "\n", " ", "\[IndentingNewLine]", 
   RowBox[{"TorsoOutput", " ", "=", " ", 
    RowBox[{"{", 
     RowBox[{
     "TorsoHipAngle", ",", "theta4", ",", "nstorso", ",", "LinearNStorso", 
      ",", "storso", ",", "LinearStorso"}], "}"}]}], ";", "\n", 
   RowBox[{
    SubscriptBox["y", 
     RowBox[{"a", ",", "2"}]], "=", 
    RowBox[{
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         RowBox[{"{", 
          RowBox[{
           RowBox[{"{", 
            RowBox[{"NSslopeOutput", "[", 
             RowBox[{"[", "2", "]"}], "]"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{
             SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "}"}], ",", 
           RowBox[{"{", 
            RowBox[{"TorsoOutput", " ", "[", 
             RowBox[{"[", "1", "]"}], "]"}], "}"}]}], "}"}], "/.", 
         "constsubs"}], "/.", "p0"}], "]"}], "\n", 
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]]}], "=", 
     RowBox[{
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", 
           RowBox[{"a", ",", "1"}]], "]"}]}], "]"}], "\[IndentingNewLine]", 
       SubscriptBox["Dy", 
        RowBox[{"a", ",", "2"}]]}], "=", 
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", 
           RowBox[{"a", ",", "2"}]], "]"}]}], "]"}], "\n", 
       RowBox[{
        SubscriptBox["DLfy", 
         RowBox[{"a", ",", "1"}]], "=", 
        RowBox[{"Simplify", "[", 
         RowBox[{
          SubscriptBox["\[PartialD]", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
          RowBox[{"Flatten", "[", 
           RowBox[{
            SubscriptBox["Dy", 
             RowBox[{"a", ",", "1"}]], ".", "d\[Chi]"}], "]"}]}], 
         "]"}]}]}]}]}]}], ";", "\[IndentingNewLine]", 
   RowBox[{
    SubscriptBox["DLfy", 
     RowBox[{"a", ",", "2"}]], "=", 
    RowBox[{"Simplify", "[", 
     RowBox[{
      SubscriptBox["\[PartialD]", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
      RowBox[{"Flatten", "[", 
       RowBox[{
        SubscriptBox["Dy", 
         RowBox[{"a", ",", "2"}]], ".", "d\[Chi]"}], "]"}]}], "]"}]}], ";"}], 
  "*)"}]], "Input",
 CellChangeTimes->{{3.513532586333204*^9, 3.513532677723161*^9}, {
   3.513532815648106*^9, 3.51353282124899*^9}, {3.513532861207259*^9, 
   3.513532869009141*^9}, {3.513532899901752*^9, 3.513532900419178*^9}, {
   3.51353311611145*^9, 3.513533116605757*^9}, 3.515521088606793*^9, {
   3.5155211643341246`*^9, 3.5155211667002597`*^9}, {3.5158526171702003`*^9, 
   3.515852621396442*^9}, {3.5158548842498703`*^9, 3.5158548875960617`*^9}, {
   3.5158551932005405`*^9, 3.5158551957126846`*^9}, {3.5158593451740203`*^9, 
   3.515859367792314*^9}, 3.51586024539751*^9, {3.5158611465420523`*^9, 
   3.515861146589055*^9}, 3.5158681409047847`*^9, {3.515869758310295*^9, 
   3.5158697588093233`*^9}, {3.515869821362901*^9, 3.515869822510967*^9}, {
   3.5158725172250957`*^9, 3.515872573737328*^9}, 3.5158863481049786`*^9, {
   3.515886571837773*^9, 3.5158865734445753`*^9}, {3.51595325150783*^9, 
   3.5159532575301747`*^9}, {3.5167083309077144`*^9, 
   3.5167083312677355`*^9}, {3.5167110381765614`*^9, 3.5167110528944035`*^9}, 
   3.5167111385673037`*^9, {3.5201093866961327`*^9, 3.520109388523237*^9}, 
   3.5201094274054613`*^9, 3.5201773866844225`*^9, {3.520437239317703*^9, 
   3.5204372535225153`*^9}, {3.5204490557876596`*^9, 
   3.5204490607579436`*^9}, {3.5207159029790354`*^9, 3.520715937276997*^9}, {
   3.527197199055368*^9, 3.527197204638042*^9}}],

Cell[BoxData[""], "Input"],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Zero", " ", "Dynamics"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", 
      "\"\<buildopt_torso\>\""}], "]"}], ";", "\n", "\[IndentingNewLine]", 
    RowBox[{"Zstatesubs", " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["z", "1"], "[", "t", "]"}], "\[Rule]", " ", 
        FormBox[
         RowBox[{"z", "[", "1", "]"}],
         TraditionalForm]}], ",", 
       RowBox[{
        RowBox[{
         SubscriptBox["z", "2"], "[", "t", "]"}], "\[Rule]", " ", 
        FormBox[
         RowBox[{"z", "[", "2", "]"}],
         TraditionalForm]}]}], "}"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["gamma", "0"], "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{"\[ScriptCapitalD]", "[", 
       RowBox[{"[", 
        RowBox[{"1", ",", ";;"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     SubscriptBox["gamma", "f"], " ", "=", " ", 
     RowBox[{"First", "[", 
      RowBox[{"(", 
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["gamma", "0"], ".", "dq"}], "]"}], ")"}], "]"}]}], ";", 
    "\n", 
    RowBox[{"Dgamma", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       SubscriptBox["\[PartialD]", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
       RowBox[{"Flatten", "[", 
        SubscriptBox["gamma", "f"], " ", "]"}]}], "]"}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<gamma_sca\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       SubscriptBox["gamma", "f"], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<gamma0_sca\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       SubscriptBox["gamma", "0"], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Dgamma_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"Dgamma", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"testgamma", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"(", 
        RowBox[{"D", "[", 
         RowBox[{
          RowBox[{
           RowBox[{"1", "/", "2"}], "*", 
           RowBox[{
            RowBox[{"Transpose", "[", "dq", "]"}], ".", "\[ScriptCapitalD]", 
            ".", "dq"}]}], ",", 
          FormBox[
           RowBox[{"dq", "[", 
            RowBox[{"[", 
             RowBox[{"1", ",", "1"}], "]"}], "]"}],
           TraditionalForm]}], "]"}], ")"}], "[", 
       RowBox[{"[", 
        RowBox[{"1", ",", "1"}], "]"}], "]"}], "]"}]}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{
     RowBox[{"TrueQ", "[", 
      RowBox[{"testgamma", "\[Equal]", " ", 
       SubscriptBox["gamma", "f"]}], "]"}], "\n", "True", "\n", 
     RowBox[{"c", " ", "=", " ", 
      RowBox[{"Simplify", "[", 
       RowBox[{
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["deltap", "hip"], "]"}]}], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "]"}]}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"H", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"Flatten", "[", 
         SubscriptBox["y", 
          RowBox[{"a", ",", "2"}]], "]"}]}], "/.", "\[InvisibleSpace]", 
       "constsubs"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"Phi", " ", "=", " ", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"{", "c", "}"}], ",", "H"}], "]"}]}], ";", "\n", 
    RowBox[{"(*", 
     RowBox[{
      RowBox[{"fbz", "=", 
       RowBox[{"{", 
        RowBox[{
         RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
         FractionBox[
          RowBox[{
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
           RowBox[{"a", "[", 
            RowBox[{"1", ",", "2"}], "]"}]}], 
          RowBox[{"a", "[", 
           RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";"}], "*)"}], 
    "\[IndentingNewLine]", 
    RowBox[{"fbz", "=", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"\[Sigma]", "[", "t", "]"}], "\[Rule]", 
       FractionBox[
        RowBox[{
         RowBox[{
          SubscriptBox["z", "1"], "[", "t", "]"}], "-", 
         RowBox[{"p", "[", "1", "]"}]}], 
        RowBox[{"a", "[", 
         RowBox[{"1", ",", "1"}], "]"}]]}], "}"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["yz", 
      RowBox[{"d", ",", "2"}]], " ", "=", " ", 
     RowBox[{
      RowBox[{"Transpose", "[", 
       RowBox[{"{", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{"HumanFunction", "[", "i", "]"}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", " ", "2", ",", "ndof"}], "}"}]}], "]"}], "}"}], 
       "]"}], "/.", "fbz"}]}], ";", "\n", 
    RowBox[{"PhiI", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["z", "1"], "[", "t", "]"}], "}"}], "}"}], ",", 
         SubscriptBox["yz", 
          RowBox[{"d", ",", "2"}]]}], " ", "]"}]}], "]"}]}], ";", "\n", 
    RowBox[{"PhidotI", " ", "=", " ", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{"Inverse", "[", "Phi", "]"}], ".", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          RowBox[{"{", 
           RowBox[{
            SubscriptBox["z", "2"], "[", "t", "]"}], "}"}], "}"}], ",", 
         RowBox[{
          RowBox[{"D", "[", 
           RowBox[{
            SubscriptBox["yz", 
             RowBox[{"d", ",", "2"}]], ",", 
            RowBox[{
             SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], "*", 
          RowBox[{
           SubscriptBox["z", "2"], "[", "t", "]"}]}]}], "]"}]}], "]"}]}], ";",
     "\n", 
    RowBox[{"zcond", " ", "=", " ", 
     RowBox[{"Table", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "i"], "[", "t", "]"}], "\[Rule]", 
        RowBox[{"PhiI", "[", 
         RowBox[{"[", 
          RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
       RowBox[{"{", 
        RowBox[{"i", ",", "1", ",", "ndof"}], "}"}]}], "]"}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Phi_inverse_mat\>\"", "]"}]}], ";", "\n", 
    
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"PhiI", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Phi_dot_inverse_mat\>\"", "]"}]}], ";", 
    "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"PhidotI", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    SubscriptBox["gamma", "0"], ";", "\n", 
    RowBox[{"Psi", " ", "=", " ", 
     RowBox[{
      RowBox[{"Simplify", "[", 
       RowBox[{"Join", "[", 
        RowBox[{
         RowBox[{"{", 
          SubscriptBox["gamma", "0"], "}"}], ",", 
         RowBox[{"H", "-", 
          RowBox[{
           RowBox[{"D", "[", 
            RowBox[{
             SubscriptBox["yz", 
              RowBox[{"d", ",", "2"}]], ",", 
             RowBox[{
              SubscriptBox["z", "1"], "[", "t", "]"}]}], "]"}], ".", 
           RowBox[{"{", "c", "}"}]}]}]}], "]"}], "]"}], "/.", "zcond"}]}], 
    ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Psi_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"Psi", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{
     SubscriptBox["kappa", "1"], "=", 
     RowBox[{"First", "[", 
      RowBox[{"c", ".", "dq"}], "]"}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["kappa", "2"], "=", 
     RowBox[{"-", 
      RowBox[{"Simplify", "[", 
       RowBox[{"D", "[", 
        RowBox[{"V", ",", 
         FormBox[
          RowBox[{"q", "[", 
           RowBox[{"[", 
            RowBox[{"1", ",", "1"}], "]"}], "]"}],
          TraditionalForm]}], "]"}], "]"}]}]}], ";", "\n", 
    RowBox[{
     SubscriptBox["f", "reduced"], " ", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        SubscriptBox["kappa", "1"], "}"}], ",", 
       RowBox[{"{", 
        SubscriptBox["kappa", "2"], "}"}]}], "}"}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<f_reduced_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       SubscriptBox["f", "reduced"], " ", "/.", "statesubs"}]}], "]"}], ";", 
    "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"z1", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"First", "[", 
        RowBox[{"c", ".", "q"}], "]"}], "\[IndentingNewLine]", "z2"}], " ", 
      "=", " ", 
      RowBox[{
       SubscriptBox["gamma", "f"], "\[IndentingNewLine]", 
       RowBox[{"zcomp", " ", "=", " ", 
        RowBox[{"{", 
         RowBox[{
          RowBox[{"{", "z1", "}"}], ",", 
          RowBox[{"{", "z2", "}"}]}], "}"}]}]}]}]}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<z_comp_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"zcomp", " ", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"z1calc", " ", "=", " ", 
     RowBox[{"First", "[", 
      RowBox[{"c", ".", "q"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"z2calc", " ", "=", " ", 
     RowBox[{"First", "[", 
      RowBox[{"c", ".", "dq"}], "]"}]}], ";", "\[IndentingNewLine]", 
    RowBox[{"zcalc", " ", "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"{", "z1calc", "}"}], ",", 
         RowBox[{"{", "z2calc", "}"}]}], "}"}], "\n", "stream"}], "=", 
      RowBox[{"OpenWrite", "[", "\"\<z_calc_mat\>\"", "]"}]}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"zcalc", " ", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Dc", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          RowBox[{"c", ".", "q"}], "]"}]}], "]"}], "\[IndentingNewLine]", 
       "DLfc"}], "=", 
      RowBox[{
       RowBox[{
        RowBox[{"Simplify", "[", 
         RowBox[{
          SubscriptBox["\[PartialD]", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{"Flatten", "[", "\[Chi]", "]"}], ",", "1"}], "}"}]], 
          RowBox[{"Flatten", "[", 
           RowBox[{"Dc", ".", "d\[Chi]"}], "]"}]}], "]"}], "\n", "stream"}], 
       "=", 
       RowBox[{"OpenWrite", "[", "\"\<DLfc_mat\>\"", "]"}]}]}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"DLfc", "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"h", "/.", "zcond"}], ";", "\n", 
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<Psi_mat\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{"Psi", "/.", "Zstatesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"SetDirectory", "[", 
     RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"Run", "[", "\"\<perl math2matopt_torso.pl\>\"", "]"}], ";", 
    "\[IndentingNewLine]", 
    RowBox[{"SetDirectory", "[", 
     RowBox[{
      RowBox[{"NotebookDirectory", "[", "]"}], "<>", 
      "\"\<buildopt_torso\>\""}], "]"}], ";"}], "*)"}]}]], "Input",
 CellChangeTimes->{{3.527197766090517*^9, 3.5271977735702133`*^9}, {
  3.527198064183445*^9, 3.527198075831641*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Inverse", " ", "Kinematics"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{"define", " ", "the", " ", "reset", " ", "map"}], "*)"}], "\n", 
  RowBox[{
   RowBox[{
    RowBox[{"\[CapitalDelta]", "=", " ", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"{", 
        RowBox[{"1", ",", "1", ",", "1", ",", 
         RowBox[{"-", "1"}], ",", 
         RowBox[{"-", "1"}]}], "}"}], ",", "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "0", ",", "1"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
       "\[IndentingNewLine]", 
       RowBox[{"{", 
        RowBox[{"0", ",", "1", ",", "0", ",", "0", ",", "0"}], "}"}]}], 
      "}"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "define", " ", "the", " ", "guard", " ", "conditions", " ", "for", " ", 
      "pre"}], "-", 
     RowBox[{"and", " ", "post"}], "-", "impact"}], "*)"}], 
   "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"initialtime", "=", 
     RowBox[{"{", 
      RowBox[{"t", "\[Rule]", "0"}], "}"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{
     "compute", " ", "the", " ", "position", " ", "of", " ", "the", " ", 
      "hip", " ", "post"}], "-", "impact"}], "*)"}], "\n", 
   RowBox[{
    RowBox[{"resetmap", "=", 
     RowBox[{"Module", "[", 
      RowBox[{
       RowBox[{"{", "newq", "}"}], ",", "\[IndentingNewLine]", 
       RowBox[{
        RowBox[{"newq", "=", 
         RowBox[{"\[CapitalDelta]", ".", 
          RowBox[{"(", "q", ")"}]}]}], ";", "\[IndentingNewLine]", 
        RowBox[{"Table", "[", 
         RowBox[{
          RowBox[{
           RowBox[{
            SubscriptBox["\[Theta]", "i"], "[", "0", "]"}], "->", 
           RowBox[{"newq", "[", 
            RowBox[{"[", 
             RowBox[{"i", ",", "1"}], "]"}], "]"}]}], ",", 
          RowBox[{"{", 
           RowBox[{"i", ",", "1", ",", 
            RowBox[{"Length", "[", "q", "]"}]}], "}"}]}], "]"}]}]}], 
      "\[IndentingNewLine]", "]"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.5158974201226616`*^9, 3.515897422208781*^9}, 
   3.5159409950429645`*^9, {3.5271981878879642`*^9, 3.52719819147633*^9}, {
   3.527198231717587*^9, 3.5271982390018187`*^9}, {3.5271983784372396`*^9, 
   3.527198378777978*^9}, 3.527198438457605*^9, {3.527199522651162*^9, 
   3.5271995251356583`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "Parameterized", " ", "Time", " ", "Using", " ", "the", " ", "Output", " ",
     "of", " ", "the", " ", "Hip"}], "*)"}], "\[IndentingNewLine]", 
  RowBox[{
   RowBox[{
    RowBox[{"phipcondition", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"{", 
        RowBox[{
         RowBox[{"p", "[", "1", "]"}], "\[Rule]", 
         SubscriptBox["p", "hip"]}], "}"}], "/.", "constsubs"}], "/.", 
      RowBox[{"t", "\[Rule]", "0"}]}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"tau", "=", 
     RowBox[{"Simplify", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{"\[Sigma]", "[", "t", "]"}], "/.", "\[InvisibleSpace]", 
          "fb"}], "/.", "\[InvisibleSpace]", "phipcondition"}], "/.", 
        "resetmap"}], "/.", "constsubs"}], "]"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"extract", " ", "the", " ", 
      RowBox[{"(", 
       RowBox[{"relative", " ", "degree", " ", "two"}], ")"}], " ", 
      "position"}], "-", 
     RowBox[{"based", " ", "outputs"}]}], "*)"}], 
   RowBox[{
    RowBox[{
     SubscriptBox["y", "2"], "=", " ", 
     RowBox[{
      RowBox[{
       RowBox[{"(", 
        RowBox[{
         SubscriptBox["y", 
          RowBox[{"a", ",", "2"}]], "  ", "-", " ", 
         SubscriptBox["y", 
          RowBox[{"d", ",", "2"}]]}], "  ", ")"}], "/.", "\[InvisibleSpace]", 
       "phipcondition"}], "/.", "constsubs"}]}], ";"}], "\n", 
   RowBox[{"(*", 
    RowBox[{
     RowBox[{"compute", " ", "the", " ", "pre"}], "-", 
     RowBox[{"and", " ", "post"}], "-", 
     RowBox[{
     "impact", " ", "values", " ", "of", " ", "the", " ", "outputs"}]}], 
    "*)"}], 
   RowBox[{
    RowBox[{"yplus", "=", 
     RowBox[{
      RowBox[{
       RowBox[{
        SubscriptBox["y", "2"], "/.", "initialtime"}], "/.", "resetmap"}], "//",
       "Simplify"}]}], ";"}], "\[IndentingNewLine]", 
   RowBox[{
    RowBox[{"yminus", "=", 
     RowBox[{
      SubscriptBox["y", "2"], "/.", "resetmap"}]}], ";"}]}]}]], "Input",
 CellChangeTimes->{{3.515889672571527*^9, 3.515889674968664*^9}, {
   3.515890444251664*^9, 3.515890444569683*^9}, {3.515942148250924*^9, 
   3.515942174667435*^9}, {3.5159426119234447`*^9, 3.5159426332196627`*^9}, 
   3.515943242212495*^9, 3.515943756731924*^9, {3.5159855129044*^9, 
   3.5159855341526146`*^9}, {3.5207282787238874`*^9, 
   3.5207282828571243`*^9}, {3.5223565377914543`*^9, 
   3.5223565440782657`*^9}, {3.5224294884521236`*^9, 
   3.5224294923883486`*^9}, {3.527198447237741*^9, 3.527198468202367*^9}, {
   3.527199112587154*^9, 3.5271991314162073`*^9}, {3.527199179103483*^9, 
   3.5271991962060127`*^9}, {3.527199339690494*^9, 3.52719934161672*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{
   "solve", " ", "for", " ", "the", " ", "inverse", " ", "kinematics"}], 
   "*)"}], "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "create", " ", "a", " ", "function", " ", "to", " ", "solve", " ", "the", 
    " ", "inverse", " ", "kinematics"}], "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"SolveEquations", "[", 
     RowBox[{"equations_", ",", "variables_", ",", "solutionnumbers_"}], 
     "]"}], ":=", "\[IndentingNewLine]", 
    RowBox[{"Module", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{"i", ",", 
        RowBox[{"solutions", "=", 
         RowBox[{"{", "}"}]}], ",", "tmpeqn", ",", "tmpsolution"}], "}"}], 
      ",", "\[IndentingNewLine]", 
      RowBox[{
       RowBox[{"For", "[", 
        RowBox[{
         RowBox[{"i", "=", "1"}], ",", 
         RowBox[{"i", "\[LessEqual]", 
          RowBox[{"Length", "[", "variables", "]"}]}], ",", 
         RowBox[{"i", "++"}], ",", "\[IndentingNewLine]", 
         RowBox[{
          RowBox[{"tmpeqn", "=", 
           RowBox[{
            RowBox[{"(", 
             RowBox[{"equations", "[", 
              RowBox[{"[", "i", "]"}], "]"}], ")"}], "\[Equal]", "0"}]}], ";",
           "\[IndentingNewLine]", 
          RowBox[{"tmpsolution", "=", 
           RowBox[{"{", 
            RowBox[{
             RowBox[{
             "variables", "\[LeftDoubleBracket]", "i", 
              "\[RightDoubleBracket]"}], "\[Rule]", 
             RowBox[{"(", 
              RowBox[{
               RowBox[{
               "variables", "\[LeftDoubleBracket]", "i", 
                "\[RightDoubleBracket]"}], "/.", 
               RowBox[{"Flatten", "[", 
                RowBox[{"{", 
                 RowBox[{"solutions", ",", 
                  RowBox[{
                   RowBox[{"Solve", "[", 
                    RowBox[{"tmpeqn", ",", 
                    RowBox[{
                    "variables", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}]}], "]"}], 
                   "\[LeftDoubleBracket]", 
                   RowBox[{
                   "solutionnumbers", "\[LeftDoubleBracket]", "i", 
                    "\[RightDoubleBracket]"}], "\[RightDoubleBracket]"}]}], 
                 "}"}], "]"}]}], ")"}]}], "}"}]}], ";", "\[IndentingNewLine]", 
          RowBox[{"solutions", "=", 
           RowBox[{"Join", "[", 
            RowBox[{"solutions", ",", 
             RowBox[{"(", 
              RowBox[{"tmpsolution", "/.", "solutions"}], ")"}]}], 
            "]"}]}]}]}], "\[IndentingNewLine]", "]"}], ";", 
       "\[IndentingNewLine]", "solutions"}]}], "\[IndentingNewLine]", "]"}]}],
    ";"}]}]], "Input",
 CellChangeTimes->{{3.5271992167871027`*^9, 3.527199235029045*^9}}],

Cell[BoxData[
 RowBox[{"(*", 
  RowBox[{
   RowBox[{"atest", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "1"}], "]"}], "\[Rule]", "1.19689396035823`"}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "2"}], "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "3"}], "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "4"}], "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"1", ",", "5"}], "]"}], "\[Rule]", "0"}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"2", ",", "1"}], "]"}], "\[Rule]", "0.20214668746590925`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"2", ",", "2"}], "]"}], "\[Rule]", "7.840374588741148`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"2", ",", "3"}], "]"}], "\[Rule]", "0.22483245436904015`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"2", ",", "4"}], "]"}], "\[Rule]", 
       RowBox[{"-", "0.8069757285872418`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"2", ",", "5"}], "]"}], "\[Rule]", "0.18712786224886793`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"3", ",", "1"}], "]"}], "\[Rule]", 
       RowBox[{"-", "0.0828259043763153`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"3", ",", "2"}], "]"}], "\[Rule]", "13.315981803707265`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"3", ",", "3"}], "]"}], "\[Rule]", "0.20729365656915424`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"3", ",", "4"}], "]"}], "\[Rule]", "4.154819403051306`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"3", ",", "5"}], "]"}], "\[Rule]", "0.2573791697685864`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"4", ",", "1"}], "]"}], "\[Rule]", 
       RowBox[{"-", "0.3808535485970228`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"4", ",", "2"}], "]"}], "\[Rule]", 
       RowBox[{"-", "10.979481912107921`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"4", ",", "3"}], "]"}], "\[Rule]", 
       RowBox[{"-", "0.19665480379455527`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"4", ",", "4"}], "]"}], "\[Rule]", 
       RowBox[{"-", "0.42141500655251124`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"4", ",", "5"}], "]"}], "\[Rule]", "0.6584527300961083`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "1"}], "]"}], "\[Rule]", 
       RowBox[{"-", "3.261238467195124`"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "2"}], "]"}], "\[Rule]", 
       RowBox[{"-", "2.0631556896285772`*^-8"}]}], ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "3"}], "]"}], "\[Rule]", "5.819659430757391`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "4"}], "]"}], "\[Rule]", "0.37746704753242283`"}], 
      ",", 
      RowBox[{
       RowBox[{"a", "[", 
        RowBox[{"5", ",", "5"}], "]"}], "\[Rule]", "3.023429578158533`"}]}], 
     "}"}]}], ";", "\n", 
   RowBox[{"A", "=", 
    RowBox[{"Table", "[", 
     RowBox[{
      RowBox[{"a", "[", 
       RowBox[{"i", ",", "j"}], "]"}], ",", 
      RowBox[{"{", 
       RowBox[{"i", ",", "5"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"j", ",", "5"}], "}"}]}], "]"}]}], ";"}], "*)"}]], "Input",
 CellChangeTimes->{{3.516708534127338*^9, 3.516708546737059*^9}, {
  3.5167085869383583`*^9, 3.516708631586912*^9}, {3.527199240969302*^9, 
  3.5271992647586412`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{"yhsolve", " ", "=", " ", 
    RowBox[{
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{"Flatten", "[", "yplus", "]"}], ",", 
       RowBox[{"{", "h", "}"}]}], "]"}], "/.", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{
        RowBox[{
         SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
        RowBox[{
         SubscriptBox["\[Theta]", "4"], "[", "t", "]"}]}], "\[Rule]", " ", 
       RowBox[{"\[Beta]", "[", "t", "]"}]}], "}"}]}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"MatrixForm", "[", "yhsolve", "]"}], ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"MatrixForm", "[", 
   RowBox[{"yhsolve", "\[LeftDoubleBracket]", 
    RowBox[{"{", 
     RowBox[{"2", ",", "3", ",", "1", ",", "5", ",", "4"}], "}"}], 
    "\[RightDoubleBracket]"}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta5Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "2", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta2Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "3", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Theta1Cond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{
       SubscriptBox["\[Theta]", "1"], "[", "t", "]"}]}], "]"}], 
    "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"ThetabetaCond", "=", 
   RowBox[{
    RowBox[{"Solve", "[", 
     RowBox[{
      RowBox[{
       RowBox[{
       "yhsolve", "\[LeftDoubleBracket]", "5", "\[RightDoubleBracket]"}], 
       "\[Equal]", "0"}], ",", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "]"}], "\[LeftDoubleBracket]", 
    "2", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Theta3Cond", "=", 
    RowBox[{
     RowBox[{"Solve", "[", 
      RowBox[{
       RowBox[{
        RowBox[{
        "yhsolve", "\[LeftDoubleBracket]", "4", "\[RightDoubleBracket]"}], 
        "\[Equal]", "0"}], ",", 
       RowBox[{
        SubscriptBox["\[Theta]", "3"], "[", "t", "]"}]}], "]"}], 
     "\[LeftDoubleBracket]", "1", "\[RightDoubleBracket]"}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       RowBox[{
        RowBox[{
         RowBox[{
          RowBox[{
           RowBox[{
            RowBox[{
             RowBox[{
              RowBox[{"Transpose", "[", 
               RowBox[{"{", 
                RowBox[{"{", 
                 RowBox[{
                  RowBox[{
                   SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], ",", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], ",", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], ",", 
                  RowBox[{
                   RowBox[{
                    SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "-", 
                   RowBox[{"\[Beta]", "[", "t", "]"}]}], ",", 
                  RowBox[{
                   SubscriptBox["\[Theta]", "5"], "[", "t", "]"}]}], "}"}], 
                "}"}], "]"}], "/.", "\[InvisibleSpace]", "Theta3Cond"}], "/.",
              "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
            "ThetabetaCond"}], "/.", "\[InvisibleSpace]", "atest"}], "/.", 
          "\[InvisibleSpace]", "Theta1Cond"}], "/.", "\[InvisibleSpace]", 
         "atest"}], "/.", "\[InvisibleSpace]", "Theta5Cond"}], "/.", 
       "\[InvisibleSpace]", "atest"}], "/.", "\[InvisibleSpace]", 
      "Theta2Cond"}], "/.", "\[InvisibleSpace]", "atest"}], ";"}], 
   "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"Theta1a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "1"], "[", "t", "]"}], "/.", "Theta1Cond"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta2a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "2"], "[", "t", "]"}], "/.", "Theta2Cond"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta3a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], "/.", "Theta3Cond"}], "/.",
     "ThetabetaCond"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta4a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     RowBox[{
      RowBox[{
       SubscriptBox["\[Theta]", "3"], "[", "t", "]"}], " ", "-", 
      RowBox[{"\[Beta]", "[", "t", "]"}]}], "/.", "Theta3Cond"}], "/.", 
    "ThetabetaCond"}]}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Theta5a", " ", "=", " ", 
   RowBox[{
    RowBox[{
     SubscriptBox["\[Theta]", "5"], "[", "t", "]"}], "/.", "Theta5Cond"}]}], 
  ";"}]}], "Input",
 CellChangeTimes->{{3.5159453782986727`*^9, 3.515945470066921*^9}, {
   3.515946921330929*^9, 3.515946925987195*^9}, 3.5159579961003695`*^9, 
   3.527198315256399*^9, {3.5271992814788837`*^9, 3.5271993118875027`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"(*", 
   RowBox[{"Velocity", " ", "of", " ", 
    RowBox[{"Outputs", ":", 
     RowBox[{"Pre", " ", "and", " ", "Post", " ", "Impact"}]}]}], "*)"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
   "compute", " ", "the", " ", "jacobian", " ", "of", " ", "the", " ", 
    "output", " ", "functions"}], "*)"}], 
  RowBox[{
   RowBox[{
    RowBox[{"Y", "=", 
     RowBox[{"Join", "[", 
      RowBox[{
       RowBox[{
        SubscriptBox["\[PartialD]", 
         RowBox[{"{", 
          RowBox[{
           RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
        RowBox[{"{", 
         SubscriptBox["p", "hip"], "}"}]}], ",", 
       RowBox[{"Simplify", "[", 
        RowBox[{
         SubscriptBox["\[PartialD]", 
          RowBox[{"{", 
           RowBox[{
            RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
         RowBox[{"Flatten", "[", 
          SubscriptBox["y", "2"], "]"}]}], "]"}]}], "]"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Yplus", "=", 
     RowBox[{
      RowBox[{
       RowBox[{"Y", "/.", "initialtime"}], "/.", "constsubs"}], "/.", 
      "resetmap"}]}], ";"}], "\n", 
   RowBox[{
    RowBox[{"Yminus", "=", 
     RowBox[{
      RowBox[{"Y", "/.", "constsubs"}], "/.", "resetmap"}]}], 
    ";"}]}]}]], "Input",
 CellChangeTimes->{{3.5158871007064247`*^9, 3.515887101970497*^9}, 
   3.5158874557787333`*^9, {3.515898875609911*^9, 3.5158988760079336`*^9}, {
   3.5158993719242983`*^9, 3.5158993862991204`*^9}, {3.515899472917075*^9, 
   3.5158994757032337`*^9}, {3.527199383615427*^9, 3.5271994241243134`*^9}}],

Cell[BoxData[
 RowBox[{
  RowBox[{"Dh", " ", "=", " ", 
   RowBox[{
    SubscriptBox["\[PartialD]", 
     RowBox[{"{", 
      RowBox[{
       RowBox[{"Flatten", "[", "q", "]"}], ",", "1"}], "}"}]], 
    RowBox[{"{", "h", "}"}]}]}], ";"}]], "Input",
 CellChangeTimes->{{3.5158876222012525`*^9, 3.5158876466726522`*^9}, {
  3.515895637521703*^9, 3.5158956394738145`*^9}, {3.52719942931901*^9, 
  3.527199451463707*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
    "\"\<buildopt_torso\>\""}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<tau\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"tau", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yplus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<y_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], 
  "\[IndentingNewLine]"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a1\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta1a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a2\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta2a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a3\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta3a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a4\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta4a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<theta_a5\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Theta5a", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_minus\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Dh", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_plus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"Yplus", "/.", "statesubs"}], "/.", "constsubs"}]}], "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<H_minus\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"Yminus", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527200201907193*^9, 3.5272002072954884`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{"FolderName", "=", 
    RowBox[{"\"\<buildopt_torso_\>\"", "<>", 
     RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";"}], 
  "\[IndentingNewLine]", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{
  "SetDirectory", "[", 
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", "\"\<perl math2matopt_torso2.pl\>\"", "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{"\"\<mkdir \>\"", "<>", "FolderName"}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{
   "\"\<cp ./buildop_torso/*.m ./\>\"", "<>", "FolderName", "<>", 
    "\"\</\>\""}], "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527199753724935*^9, 3.527199753734995*^9}, {
   3.527200017992285*^9, 3.527200073839613*^9}, {3.527200111322361*^9, 
   3.527200144104334*^9}, 3.527200387814547*^9, {3.527200440691627*^9, 
   3.5272004544796963`*^9}}],

Cell[BoxData[{
 RowBox[{
  RowBox[{"SetDirectory", "[", 
   RowBox[{
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "<>", 
    "\"\<build_torso\>\""}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<jpos_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"pos", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<D_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<C_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalC]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<G_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalG]", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<De_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalD]e", "/.", "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<E_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"\[ScriptCapitalE]", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_sca\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"h", "/.", "statesubs"}]}], "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<h_dot_mat\>\"", "]"}]}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{"hdot", "/.", "statesubs"}]}], "]"}], 
  ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hip"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaphip_sca\>\"", "]"}]}], ";", "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["deltap", "hip"], "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<phip_dot_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["p", "hipdot"], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{
   RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{"stream", "=", 
     RowBox[{"OpenWrite", "[", "\"\<deltaphip_dot_mat\>\"", "]"}]}], ";", 
    "\n", 
    RowBox[{"Write", "[", 
     RowBox[{"stream", ",", 
      RowBox[{
       RowBox[{
        SubscriptBox["deltap", "hipdot"], " ", "/.", "\[InvisibleSpace]", 
        "constsubs"}], "/.", "statesubs"}]}], "]"}], ";", "\n", 
    RowBox[{"Close", "[", "stream", "]"}], ";", "\n", 
    RowBox[{"Clear", "[", "stream", "]"}], ";"}], "*)"}]}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<yd2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfyd2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"d", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya1_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<ya2_vec\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["y", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<Dya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["Dy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya1_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "1"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<DLfya2_mat\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{
      SubscriptBox["DLfy", 
       RowBox[{"a", ",", "2"}]], "/.", "\[InvisibleSpace]", "constsubs"}], "/.",
      "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"stream", "=", 
   RowBox[{"OpenWrite", "[", "\"\<sigma_sca\>\"", "]"}]}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Write", "[", 
   RowBox[{"stream", ",", 
    RowBox[{
     RowBox[{"\[Sigma]y", "/.", "\[InvisibleSpace]", "constsubs"}], "/.", 
     "statesubs"}]}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Close", "[", "stream", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Clear", "[", "stream", "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527199539760606*^9, 3.5271995402923*^9}, 
   3.52719985344182*^9}],

Cell[BoxData[{
 RowBox[{
  RowBox[{
   RowBox[{"FolderName", "=", 
    RowBox[{"\"\<build_torso_\>\"", "<>", 
     RowBox[{"ToString", "[", "LineNumber", "]"}]}]}], ";"}], "\n", 
  RowBox[{"(*", 
   RowBox[{
    RowBox[{
    "SetDirectory", "[", 
     "\"\</home/shu/workspace/Research/2DKnee_Torso_Outputs\>\"", "]"}], 
    ";"}], "*)"}]}], "\[IndentingNewLine]", 
 RowBox[{
  RowBox[{
  "SetDirectory", "[", 
   "\"\</home/shu/workspace/Research/AMBER_Output_Test/model/\>\"", "]"}], 
  ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", "\"\<perl math2mat_torso.pl\>\"", "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{"\"\<mkdir \>\"", "<>", "FolderName"}], "]"}], ";"}], "\n", 
 RowBox[{
  RowBox[{"Run", "[", 
   RowBox[{
   "\"\<cp ./build_torso/*.m ./\>\"", "<>", "FolderName", "<>", "\"\</\>\""}],
    "]"}], ";"}]}], "Input",
 CellChangeTimes->{{3.527199753724935*^9, 3.527199753734995*^9}, {
   3.527200017992285*^9, 3.527200073839613*^9}, {3.527200111322361*^9, 
   3.527200144104334*^9}, 3.527200387814547*^9}]
},
WindowSize->{959, 1026},
WindowMargins->{{Automatic, -9}, {Automatic, -8}},
ShowSelection->True,
Magnification->1.100000023841858,
FrontEndVersion->"8.0 for Linux x86 (64-bit) (November 7, 2010)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[545, 20, 1084, 24, 108, "Input"],
Cell[1632, 46, 2757, 63, 184, "Input"],
Cell[4392, 111, 7525, 219, 575, "Input"],
Cell[11920, 332, 33970, 993, 1615, "Input"],
Cell[45893, 1327, 15399, 435, 1032, "Input"],
Cell[61295, 1764, 9064, 279, 653, "Input"],
Cell[70362, 2045, 2227, 64, 185, "Input"],
Cell[72592, 2111, 3416, 106, 181, "Input"],
Cell[76011, 2219, 4494, 111, 188, "Input"],
Cell[80508, 2332, 26, 0, 31, "Input"],
Cell[80537, 2334, 13443, 362, 1571, "Input"],
Cell[93983, 2698, 2630, 68, 279, "Input"],
Cell[96616, 2768, 2717, 72, 165, "Input"],
Cell[99336, 2842, 2744, 69, 260, "Input"],
Cell[102083, 2913, 3850, 112, 184, "Input"],
Cell[105936, 3027, 5531, 159, 298, "Input"],
Cell[111470, 3188, 1602, 45, 109, "Input"],
Cell[113075, 3235, 417, 11, 33, "Input"],
Cell[113495, 3248, 4474, 135, 906, "Input"],
Cell[117972, 3385, 1103, 32, 127, "Input"],
Cell[119078, 3419, 11505, 348, 1942, "Input"],
Cell[130586, 3769, 1035, 29, 127, "Input"]
}
]
*)

(* End of internal cache information *)
